#include "vision.hpp"
#include "math.h"

Vision::Vision(Detector* detector, Tracker* tracker, cv::Size stdSize, double retargetOnSpeed)
{
	this->detector = detector;
	this->tracker = tracker;
	this->stdSize = stdSize;
	this->retargetOnSpeed = retargetOnSpeed;
}

double Vision::geometricalDistance(int x, int y) {
	return sqrt(x * x + y * y);
}

void Vision::capture(cv::Mat& frame)
{
	this->frame = frame;
	this->width = frame.cols;
	this->height = frame.rows;
	this->halfWidth = width / 2.0f;
	this->halfHeight = height / 2.0f;
}

bool Vision::spot()
{
	if (isLost || trackingDeltaS < retargetOnSpeed) {
		detectionResults = detector->detect(frame);
		if (detectionResults.size() > 0) {
			min_ds = sqrt(halfWidth * halfWidth + halfHeight * halfHeight);
			for (int i = 0; i < detectionResults.size(); i++) {
				tmp_ds = geometricalDistance(detectionResults[i].x - halfWidth, detectionResults[i].y - halfHeight);
				if (tmp_ds < min_ds) {
					min_ds = tmp_ds;
					mostCenteredDetectionResult = detectionResults[i];
				}
			}
			target = mostCenteredDetectionResult; //consider the most centered result
			tracker->init(frame, target); //important
			isLost = false;
		} else {
			isLost = true;
		}
	} else {
		lastTrackingResult = thisTrackingResult;
		thisTrackingResult = tracker->update(frame);
		trackingDeltaX = thisTrackingResult.x - lastTrackingResult.x;
		trackingDeltaY = thisTrackingResult.y = lastTrackingResult.y;
		trackingDeltaS = geometricalDistance(trackingDeltaX, trackingDeltaY);
		target = thisTrackingResult;
	}
	dx = target.x - halfWidth;
	dy = target.y - halfHeight;
	dz = target.area() / stdSize.area();
	ds = geometricalDistance(dx, dy);

	if (!isLost) {
		rectangle(frame, cv::Point(result.x, result.y), Point(result.x + result.width, result.y + result.height), cv::Scalar(0, 255, 255),
	}
	return !isLost;
}

void Vision::manualRetargetMouseHandler(int event, int x, int y, int flags, void *param)
{
	switch (event)
	{

	case CV_EVENT_MOUSEMOVE:
		if (drawbox) {
			box.width = x - box.x;
			box.height = y - box.y;
		}
		break;
	case CV_EVENT_LBUTTONDOWN:
		drawbox = true;
		box = Rect(x, y, 0, 0);
		break;
	case CV_EVENT_LBUTTONUP:
		drawbox = false;
		if (box.width < 0) {
			box.x += box.width;
			box.width *= -1;
		}
		if (box.height < 0) {
			box.y += box.height;
			box.height *= -1;
		}
		getbox = true;
		break;
	}
}
